/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 *
 * File: foc.c
 *
 * Code generated for Simulink model 'foc'.
 *
 * Model version                  : 9.71
 * Simulink Coder version         : 24.2 (R2024b) 21-Jun-2024
 * C/C++ source code generated on : Sun Mar 30 09:06:55 2025
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: ARM Compatible->ARM Cortex-M
 * Code generation objectives:
 *    1. Execution efficiency
 *    2. RAM efficiency
 * Validation result: Not run
 */

#include "foc.h"
#include <math.h>
#include <stdint.h>

/* Block signals and states (default storage) */
DW rtDW;

/* External inputs (root inport signals with default storage) */
ExtU rtU;

/* External outputs (root outports fed by signals with default storage) */
ExtY rtY;

/* Real-time model */
static RT_MODEL rtM_;
RT_MODEL *const rtM = &rtM_;
extern float rt_hypotf(float u0, float u1);
float rt_hypotf(float u0, float u1)
{
  float a;
  float b;
  float y;
  a = fabsf(u0);
  b = fabsf(u1);
  if (a < b) {
    a /= b;
    y = sqrtf(a * a + 1.0F) * b;
  } else if (a > b) {
    b /= a;
    y = sqrtf(b * b + 1.0F) * a;
  } else {
    y = a * 1.41421354F;
  }

  return y;
}

/* Model step function */
void foc_step(void)
{
  float rtb_Add4;
  float rtb_DeadZone;
  float rtb_DeadZone_l;
  float rtb_Gain1;
  float rtb_IntegralGain;
  float rtb_IntegralGain_a;
  float rtb_Product2;
  float rtb_add_b;
  float rtb_add_c;
  int8_t tmp;
  int8_t tmp_0;

  /* Outputs for Atomic SubSystem: '<S1>/FOC' */
  /* Outputs for Atomic SubSystem: '<S11>/Two phase CRL wrap' */
  /* Gain: '<S12>/one_by_sqrt3' incorporates:
   *  Inport: '<Root>/ia'
   *  Inport: '<Root>/ib'
   *  Sum: '<S12>/a_plus_2b'
   */
  rtb_Gain1 = ((rtU.ia + rtU.ib) + rtU.ib) * 0.577350259F;

  /* Product: '<S2>/Product' incorporates:
   *  AlgorithmDescriptorDelegate generated from: '<S12>/a16'
   *  Constant: '<S2>/Zero'
   *  Inport: '<Root>/cosine'
   *  Inport: '<Root>/enable'
   *  Inport: '<Root>/ia'
   *  Inport: '<Root>/sine'
   *  Product: '<S10>/Product4'
   *  Product: '<S10>/Product5'
   *  Sum: '<S10>/Add3'
   *  Sum: '<S2>/Add'
   */
  rtb_IntegralGain = (0.0F - (rtU.cosine * rtU.ia + rtU.sine * rtb_Gain1)) *
    rtU.enable;

  /* End of Outputs for SubSystem: '<S11>/Two phase CRL wrap' */

  /* Sum: '<S59>/Sum' incorporates:
   *  DiscreteIntegrator: '<S50>/Integrator'
   *  Gain: '<S55>/Proportional Gain'
   */
  rtb_DeadZone = 0.07F * rtb_IntegralGain + rtDW.Integrator_DSTATE;

  /* Saturate: '<S57>/Saturation' */
  if (rtb_DeadZone > 1.0F) {
    rtb_add_b = 1.0F;
  } else if (rtb_DeadZone < -1.0F) {
    rtb_add_b = -1.0F;
  } else {
    rtb_add_b = rtb_DeadZone;
  }

  /* End of Saturate: '<S57>/Saturation' */

  /* Outputs for Atomic SubSystem: '<S11>/Two phase CRL wrap' */
  /* Sum: '<S10>/Add4' incorporates:
   *  AlgorithmDescriptorDelegate generated from: '<S12>/a16'
   *  Inport: '<Root>/cosine'
   *  Inport: '<Root>/ia'
   *  Inport: '<Root>/sine'
   *  Product: '<S10>/Product6'
   *  Product: '<S10>/Product7'
   */
  rtb_Add4 = rtb_Gain1 * rtU.cosine - rtU.sine * rtU.ia;

  /* End of Outputs for SubSystem: '<S11>/Two phase CRL wrap' */

  /* Product: '<S2>/Product1' incorporates:
   *  Inport: '<Root>/enable'
   *  Inport: '<Root>/idcRef'
   *  Sum: '<S2>/Add1'
   */
  rtb_IntegralGain_a = (rtU.idcRef - rtb_Add4) * rtU.enable;

  /* Sum: '<S113>/Sum' incorporates:
   *  DiscreteIntegrator: '<S104>/Integrator'
   *  Gain: '<S109>/Proportional Gain'
   */
  rtb_DeadZone_l = 0.07F * rtb_IntegralGain_a + rtDW.Integrator_DSTATE_d;

  /* Saturate: '<S111>/Saturation' */
  if (rtb_DeadZone_l > 1.0F) {
    rtb_Gain1 = 1.0F;
  } else if (rtb_DeadZone_l < -1.0F) {
    rtb_Gain1 = -1.0F;
  } else {
    rtb_Gain1 = rtb_DeadZone_l;
  }

  /* End of Saturate: '<S111>/Saturation' */

  /* Sum: '<S9>/Add3' incorporates:
   *  Inport: '<Root>/cosine'
   *  Inport: '<Root>/sine'
   *  Product: '<S9>/Product4'
   *  Product: '<S9>/Product5'
   */
  rtb_Product2 = rtU.cosine * rtb_add_b - rtU.sine * rtb_Gain1;

  /* Sum: '<S9>/Add4' incorporates:
   *  Inport: '<Root>/cosine'
   *  Inport: '<Root>/sine'
   *  Product: '<S9>/Product6'
   *  Product: '<S9>/Product7'
   */
  rtb_Gain1 = rtU.sine * rtb_add_b + rtb_Gain1 * rtU.cosine;

  /* Fcn: '<S3>/x->r' */
  rtb_add_b = rt_hypotf(rtb_Product2, rtb_Gain1);

  /* Switch: '<S2>/Switch' incorporates:
   *  Constant: '<S2>/Constant'
   *  Constant: '<S5>/Constant'
   *  RelationalOperator: '<S5>/Compare'
   *  Saturate: '<S2>/Saturation'
   */
  if (rtb_add_b < 1.0F) {
    rtb_add_b = 1.0F;
  } else if (rtb_add_b > 10.0F) {
    /* Saturate: '<S2>/Saturation' */
    rtb_add_b = 10.0F;
  }

  /* End of Switch: '<S2>/Switch' */

  /* Product: '<S2>/Product2' */
  rtb_Product2 /= rtb_add_b;

  /* Gain: '<S13>/one_by_two' */
  rtb_add_c = 0.5F * rtb_Product2;

  /* Gain: '<S13>/sqrt3_by_two' incorporates:
   *  Product: '<S2>/Product3'
   */
  rtb_Gain1 = rtb_Gain1 * rtb_add_b * 0.866025388F;

  /* Sum: '<S13>/add_b' */
  rtb_add_b = rtb_Gain1 - rtb_add_c;

  /* Sum: '<S13>/add_c' */
  rtb_add_c = (0.0F - rtb_add_c) - rtb_Gain1;

  /* Gain: '<S2>/Gain1' incorporates:
   *  MinMax: '<S2>/Min'
   *  MinMax: '<S2>/Min1'
   *  Sum: '<S2>/Add4'
   */
  rtb_Gain1 = (fminf(fminf(rtb_Product2, rtb_add_b), rtb_add_c) + fmaxf(fmaxf
    (rtb_Product2, rtb_add_b), rtb_add_c)) * 0.5F;

  /* DeadZone: '<S96>/DeadZone' */
  if (rtb_DeadZone_l > 1.0F) {
    rtb_DeadZone_l--;
  } else if (rtb_DeadZone_l >= -1.0F) {
    rtb_DeadZone_l = 0.0F;
  } else {
    rtb_DeadZone_l++;
  }

  /* End of DeadZone: '<S96>/DeadZone' */

  /* Gain: '<S101>/Integral Gain' */
  rtb_IntegralGain_a *= 0.06F;

  /* DeadZone: '<S42>/DeadZone' */
  if (rtb_DeadZone > 1.0F) {
    rtb_DeadZone--;
  } else if (rtb_DeadZone >= -1.0F) {
    rtb_DeadZone = 0.0F;
  } else {
    rtb_DeadZone++;
  }

  /* End of DeadZone: '<S42>/DeadZone' */

  /* Gain: '<S47>/Integral Gain' */
  rtb_IntegralGain *= 0.06F;

  /* Switch: '<S40>/Switch1' incorporates:
   *  Constant: '<S40>/Clamping_zero'
   *  Constant: '<S40>/Constant'
   *  Constant: '<S40>/Constant2'
   *  RelationalOperator: '<S40>/fix for DT propagation issue'
   */
  if (rtb_DeadZone > 0.0F) {
    tmp = 1;
  } else {
    tmp = -1;
  }

  /* Switch: '<S40>/Switch2' incorporates:
   *  Constant: '<S40>/Clamping_zero'
   *  Constant: '<S40>/Constant3'
   *  Constant: '<S40>/Constant4'
   *  RelationalOperator: '<S40>/fix for DT propagation issue1'
   */
  if (rtb_IntegralGain > 0.0F) {
    tmp_0 = 1;
  } else {
    tmp_0 = -1;
  }

  /* Switch: '<S40>/Switch' incorporates:
   *  Constant: '<S40>/Clamping_zero'
   *  Constant: '<S40>/Constant1'
   *  Logic: '<S40>/AND3'
   *  RelationalOperator: '<S40>/Equal1'
   *  RelationalOperator: '<S40>/Relational Operator'
   *  Switch: '<S40>/Switch1'
   *  Switch: '<S40>/Switch2'
   */
  if ((rtb_DeadZone != 0.0F) && (tmp == tmp_0)) {
    rtb_IntegralGain = 0.0F;
  }

  /* Update for DiscreteIntegrator: '<S50>/Integrator' incorporates:
   *  Switch: '<S40>/Switch'
   */
  rtDW.Integrator_DSTATE += rtb_IntegralGain;

  /* Switch: '<S94>/Switch1' incorporates:
   *  Constant: '<S94>/Clamping_zero'
   *  Constant: '<S94>/Constant'
   *  Constant: '<S94>/Constant2'
   *  RelationalOperator: '<S94>/fix for DT propagation issue'
   */
  if (rtb_DeadZone_l > 0.0F) {
    tmp = 1;
  } else {
    tmp = -1;
  }

  /* Switch: '<S94>/Switch2' incorporates:
   *  Constant: '<S94>/Clamping_zero'
   *  Constant: '<S94>/Constant3'
   *  Constant: '<S94>/Constant4'
   *  RelationalOperator: '<S94>/fix for DT propagation issue1'
   */
  if (rtb_IntegralGain_a > 0.0F) {
    tmp_0 = 1;
  } else {
    tmp_0 = -1;
  }

  /* Switch: '<S94>/Switch' incorporates:
   *  Constant: '<S94>/Clamping_zero'
   *  Constant: '<S94>/Constant1'
   *  Logic: '<S94>/AND3'
   *  RelationalOperator: '<S94>/Equal1'
   *  RelationalOperator: '<S94>/Relational Operator'
   *  Switch: '<S94>/Switch1'
   *  Switch: '<S94>/Switch2'
   */
  if ((rtb_DeadZone_l != 0.0F) && (tmp == tmp_0)) {
    rtb_IntegralGain_a = 0.0F;
  }

  /* Update for DiscreteIntegrator: '<S104>/Integrator' incorporates:
   *  Switch: '<S94>/Switch'
   */
  rtDW.Integrator_DSTATE_d += rtb_IntegralGain_a;

  /* Outport: '<Root>/S' incorporates:
   *  Constant: '<S2>/Constant1'
   *  Constant: '<S2>/Constant2'
   *  Gain: '<S2>/Gain'
   *  Sum: '<S2>/Add2'
   *  Sum: '<S2>/Add3'
   */
  rtY.S[0] = (rtb_Product2 + rtb_Gain1) * 0.5F + 0.5F;

  /* End of Outputs for SubSystem: '<S1>/FOC' */
  rtY.S[1] = 1.0F;

  /* Outputs for Atomic SubSystem: '<S1>/FOC' */
  rtY.S[2] = (rtb_add_b + rtb_Gain1) * 0.5F + 0.5F;

  /* End of Outputs for SubSystem: '<S1>/FOC' */
  rtY.S[3] = 1.0F;

  /* Outputs for Atomic SubSystem: '<S1>/FOC' */
  rtY.S[4] = (rtb_add_c + rtb_Gain1) * 0.5F + 0.5F;

  /* End of Outputs for SubSystem: '<S1>/FOC' */
  rtY.S[5] = 1.0F;

  /* Outport: '<Root>/idc' */
  rtY.idc = rtb_Add4;
}

/* Model initialize function */
void foc_initialize(void)
{
  /* (no initialization code required) */
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */
